#ifndef CLASS_GOAL_FLYTO
#define CLASS_GOAL_FLYTO

#include "goal.h"
#include <coordMath.h>
#include <time.h>
#include <sys/types.h>

namespace spsu_art {

class goal_flyto {
private:
    //where we are going
    NEDvector dest;

    //this goal should receve an acknowlage telegram pretty quickly
    bool waiting_for_ack;

    //if we dont get the acknowlage by this time we should replan
    time_t ack_timeout;

    //A estimated time of arrival to our position. if we excede this time report goal failed.
    //used to figure out if we are making progress toward our goal
    time_t arrival_time;

    //the time constant added to the current time 
    //this is how long the autopilot has to respond
    //to a flyto with its ack packet or something is wrong
    const time_t timeout_interval = 3;

    //used to check if two floating points are equal to each other.
    //if two positions are within a mm we will consiter them the same postition. 
    const double fudge_factor = 0.0001;

public:
    goal_flyto(NEDvector coord) : dest(coord) { }
    goal_flyto(LLHvector coord);

    void activate();
    g_status process();

    //this goal has nothing to clean up 
    void terminate() { }

    bool handle_message(Telegram& message);

} //end class goal_flyto

} //end namspace spsu_art

#endif
